#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import re
import os

import threading
from pid import pid
import threading
import time
import random
from stepmotor import stepMotor
import matplotlib.pyplot as plt
'''
import logging
from loggercfg import loggerCfgConsoleHandler
from loggercfg import loggerCfgFileHandler
from loggercfg import loggerCfgFileAndConsoleHandler

loggerc = loggerCfgConsoleHandler()
loggerf = loggerCfgFileHandler()
loggerfc = loggerCfgFileAndConsoleHandler()
'''

motor = stepMotor(0.7, 0.25, 0.05, 500, 10, 100) #增量式参数
#motor = stepMotor(0.9, 0.01, 0.1, 300, 300, 10) #位置式参数

#线程退出条件
thread_stop = 1
thread_stop2 = 1

'''
* functionname : thread_pid_set
* para()       :
* return       : 
* discription  : 
模拟pid调节频率
* author       : zhongmingsen
* date         : 2023-06-14 
'''
def thread_pid_set() :
    #仿真代码，用于绘制图形
    
    while True :
        #模拟线程结束
        if 0 == thread_stop :
            break
        '''
        #time = input("##########################################请输入用时 : ")
        dst =  input("##########################################请输入位置 : ")

        motor.stepMotorDstTime(int(dst) * 1000, 3)
        #motor.stepMotorDst(int(dst) * 1000)
        print('#############设置目标位置和时间##################', motor.p_dst, motor.p_src, motor.time)
        '''
        
        r = random.uniform(0,1)
        r = r * 2 - 1
        r2 = random.uniform(0,1)
        motor.stepMotorDstTime(int(r * 60) * 1000, int(1 + r2)) 
        print('#############设置目标位置和时间##################', motor.p_dst, motor.p_src, motor.time)
        time.sleep(2)
        
        
    motor.stepMotorDstTime(int(0) * 1000, 3)
    #print('#############设置目标位置和时间##################', motor.p_dst, motor.p_src, motor.time)

'''
* functionname : thread_pid_adjust
* para()       :
* return       : 
* discription  : 
模拟pid调节频率
* author       : zhongmingsen
* date         : 2023-06-14 
'''
def thread_pid_adjust() :

    while True :
        #模拟线程结束
        if 0 == thread_stop2 :
            break
        #模拟步进电机的PWM脉冲数,实际应该在PWM中断中motor.p_src计数
        if 0 != motor.feedbackFreq :
            r = random.uniform(0.9, 1)
            r = round(r + 0.05, 6) # 0.95 ~ 1.05
            motor.p_src = motor.p_src + motor.feedbackFreq * r
            motor.p_src = int(motor.p_src)
            #编码器闭环检测速度，此变化引入，否则PWM自闭环，则输出的速度即认为实际速度
            #motor.feedbackFreq = int(motor.feedbackFreq * r)
            
        #--------------------------步进电机开始调节------------------------------------------
        motor.stepMotorPidAdjust()
        #--------------------------步进电机结束调节------------------------------------------   
        time.sleep(motor.ts)
'''
* functionname : threadTest
* para()       :
* return       : 
* discription  : 
模拟pid调节频率
* author       : zhongmingsen
* date         : 2023-06-14 
'''
def threadTest() :
    global thread_stop
    global thread_stop2
    #设置步进电机指定时间从初始位置到达目标位置，同时允许PID调节 
    motor.stepMotorPara(0,0,0)
    motor.stepMotorPara(0,5*10000,3)

    #启动一个定时线程
    thread1 = threading.Thread(target = thread_pid_set, args=())
    thread1.start()
    thread2 = threading.Thread(target = thread_pid_adjust, args=())
    thread2.start()
    #等待步进电机完成第一次调节
    cnt = 0
    while cnt <= 30 :
        cnt = cnt + 1
        print('------------------------------------------------运行秒数', cnt)
        time.sleep(1)
    '''
    print('-----------------------------------------', motor.p_src, motor.p_dst, motor.feedbackFreq, motor.time)
    #在步进电机运动状态，设置指定时间到达目标位置
    motor.stepMotorPdst(500, 5, 0)
    print('-----------------------------------------', motor.p_src, motor.p_dst, motor.feedbackFreq, motor.time)
    
    #等待步进电机完成第二次调节
    cnt = 0
    while cnt < 12 :
        cnt = cnt + 1
        time.sleep(1)
    '''
    
    #等待2S，等待线程结束
    thread_stop = 0
    cnt = 0
    while cnt < 10 :
        cnt = cnt + 1  
        time.sleep(1)
    thread_stop2 = 0
    cnt = 0
    while cnt < 2 :
        cnt = cnt + 1  
        time.sleep(1)

    #绘制可视化图形
    print('--目标位置和当前位置--', motor.p_dst, motor.p_src, motor.feedbackFreqMax, motor.deltaSrcDstMax)
    x = list(range(0,len(motor.feedbackFreqList)))
    
    plt.plot(x, motor.feedbackFreqList, 'b')
    plt.plot(x, motor.targetFreqList, 'r')
    plt.plot(x, motor.srcDisList, 'g')
    plt.plot(x, motor.dstDisList, 'r--')
    plt.plot(x, motor.timeList, 'y')
    
    #plt.plot(x, motor.remainingDistanceList, 'c-.')  
    #plt.plot(x, motor.remainingDistanceMaxList, 'm-.') 
    
    plt.show()
    
'''
* function name : main
* para()        : 
* return        : 
* description   : 用于测试
* author        : zhongmingsen
* date          : 2023.4.1
'''                          
def main(argv=None):
    if argv is None:
        argv = sys.argv
        if len(argv) < 1:
            print('please set dbc file name!')
        else:                                           
            threadTest()
            print('get dbc signal tabel success!')   

if __name__ == "__main__":
    sys.exit(main())
